/*
 * Copyright (C) 2006 Roman Krashanitsa
 *
 * @author Roman Krashanitsa
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, write to
 * the Free Software Foundation, 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA. 
 *
 */

package pprz.data_components;

import java.io.IOException;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;

import net.jscience.math.MathFP;
import pprz.Link;
import pprz.data_components.events.DownlinkMessageEvent;
import pprz.data_components.parsers.MessageParser.MessageFormat;


public class PprzMsgProtocol extends MsgProtocol implements UplinkProtocol 
{
	
	protected Message message; 
	protected MessageFormat format;
	protected Link link;
	
	public PprzMsgProtocol(Link link)
	{
		super();
		message=new Message();
		this.link=link;
	}
	
	//decode byte array to an aircraft info structure, wrap it in a message and send to GUI
	public void decode(byte[] cc) 
	{
		if (cc.length<2) return; //check length
		
		int id=cc[0]; //sender id
		int msgid=cc[1]; //msg id
		
		if (!link.getMessageFormats().containsKey(0*0x100000000l+msgid)) return;
		//else if format exists --->
		//Fill in known Aircraft structures
		Aircraft a;
		if (!link.getAircraftList().containsKey(id)) {
			link.getAircraftList().put(id, new Aircraft(id,link.getAircraftList().size()));
		}
		a=link.getAircraftList().get(id);
		a.lastMSG=0;
		
		if (cc.length > 2) {
			ByteBuffer b = ByteBuffer.wrap(cc, 2, cc.length - 2);
			b.order(ByteOrder.LITTLE_ENDIAN);

			format=link.getMessageFormats().get(0*0x100000000l+msgid);
			if (format.noName || format.noClass || format.noId || 
				format.noTypeField>0 || format.noNameField>0) return;
			//else format is ok --->
			message.setFieldsLength(format.fields.size());
			Long[] fld;
			for (int i=0;i<message.getFields().size();i++)
			{
				switch (format.fields.get(i).type)
				{
				case 0://int8,uint8
				case 1:
					fld=new Long[1];
					fld[0]=MathFP.toFP(b.get());
					message.setField(i, fld);
					break;
				case 2://int16
				case 3://uint16
					fld=new Long[1];
					fld[0]=MathFP.toFP(b.getShort());
					message.setField(i, fld);
					break;
				case 4://int32
				case 5://uint32
					fld=new Long[1];
					fld[0]=MathFP.toFP(b.getInt());
					message.setField(i, fld);
					break;
				case 6://float
					fld=new Long[1];
					fld[0]=MathFP.toFP((long)(b.getFloat()*1E8));
					message.setField(i, fld);
					break;
				case 7://string
					break;
				case 8: // uint16[]
				case 9: // int16[]
					int n=(cc.length-2-1)/2; // CORRECT THIS / clean format
					fld=new Long[n];
					for (int j=0;j<n;j++)
					{
						fld[j]=MathFP.toFP(b.getShort());
					}
					message.setField(i, fld);
					break;
				default: 
					System.out.println(this.getClass().toString()+": invalid message field type.");
				}
			}
			
			b.rewind();
			b.position(2);
			a.lastMSG=msgid;		
			switch (msgid) {
				case 6: {
					//set roll, heading, pitch
					a.set_roll(b.getShort());
					a.set_yaw(b.getShort());
					a.set_pitch(b.getShort());
					break;
				}
				case 8: {
					a.set_gps_mode(b.get());
					a.set_gps_utm_east(b.getInt()/100.f);
					a.set_gps_utm_north(b.getInt()/100.f);
					a.set_gps_course(b.getShort()/10.f);
					a.set_gps_alt(b.getInt()/100.f);
					a.set_gps_vel(b.getShort()/100.f);
					a.set_gps_climb(b.getShort()/100.f);
					a.set_gps_itow(b.getInt());
					a.set_gps_utm_zone(b.get());
					a.set_gps_nb_err(b.get());
					break;				
				}
				case 10: {
					a.set_cur_block(b.get());
					a.set_cur_stage(b.get());
					b.position(6); //scroll to desired_course
					a.set_desired_course(b.getShort()/100.f);
					a.set_dist2_wp(b.getFloat());
					a.set_dist2_home(b.getFloat());
					break;
				}
				case 11: {
					a.set_ap_mode(b.get());
					a.set_ap_gaz(b.get());
					a.set_ap_lateral(b.get());
					a.set_ap_horizontal(b.get());
					a.set_if_calib_mode(b.get());
					a.set_mcu1_status(b.get());
					a.set_lls_calib(b.get());
					break;				
				}
				case 12: {
					a.set_throttle(b.getShort()/9600.f);
					a.set_voltage(b.get()/10.f); 
					a.set_flight_time(b.getShort());
					a.set_kill_auto_throttle(b.get());
					a.set_block_time(b.getShort());
					a.set_stage_time(b.getShort());
					a.set_energy(b.getShort());
					break;				
				}
				case 16: {
					a.set_des_roll(b.getFloat());
					a.set_des_pitch(b.getFloat());
					a.set_des_x(b.getFloat());
					a.set_des_y(b.getFloat());
					a.set_des_altitude(b.getFloat());
					a.set_des_climb(b.getFloat());
					break;
				}
					
				case 23: {
					b.position(16);
					a.set_rx_bytes_rate(b.getFloat());
					a.set_rx_msgs_rate(b.getFloat());
					break;				
				}
				default: {
					break;
				}
			}
			message.setClass((byte)0);
			message.setId(msgid);
			message.setTime(System.currentTimeMillis());
//			if (a.state==Aircraft.NOTHING) { // not aircraft message, pass it on
				message.setPayload(cc); 
//			}
//			else { // aircraft message, appropriate fields are filled
				message.setPayload(a); 
//			}			
			fireDownlinkMessageEvent(new DownlinkMessageEvent(this,message));
		}
		else return; //message is too short, skip it
	}
	
	public void encode_acinfo(UplinkDevice d, int acId, float utm_east, float utm_north, 
						  float alt, float course, float speed) 
	{
//		    uint8_t id = DL_ACINFO_ac_id(dl_buffer);
//		    float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer));
//		    float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer));
//		    float a = MOfCm(DL_ACINFO_alt(dl_buffer));
//		    float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.);
//		    float s = MOfCm(DL_ACINFO_speed(dl_buffer));
//		    SetAcInfo(id, ux, uy, c, a, s);
	}
	public void encode_move_wp(UplinkDevice d, int acId, int wpId, 
						   float utm_east, float utm_north, float alt)
	{
//			    float a = MOfCm(DL_MOVE_WP_alt(dl_buffer));
//		    MoveWaypoint(wp_id, ux, uy, a);
//		    DOWNLINK_SEND_WP_MOVED(&wp_id, &ux, &uy, &a);
	} 
	public void encode_goto_block(UplinkDevice d, int acId, int blockId)
	{    
//		if (msg_id == DL_BLOCK) {
//			    nav_goto_block(DL_BLOCK_block_id(dl_buffer));
	}
		
	public void encode_wind_info(UplinkDevice d, int acId, float east, float north) 
	{
//				else if (msg_id == DL_WIND_INFO) {
//		    wind_east = DL_WIND_INFO_east(dl_buffer);
//		    wind_north = DL_WIND_INFO_north(dl_buffer);
	} 
				
	public void encode_telemetry_mode(UplinkDevice d, int acId, int mode)
	{
//		if (msg_id == DL_TELEMETRY_MODE) 
//		    telemetry_mode_Ap = DL_TELEMETRY_MODE_mode(dl_buffer);
//		#endif /** AP */
	} 
	public void encode_hitl_ir(UplinkDevice d, int acId, float ir_roll, float ir_pitch)
	{
//			else if (msg_id == DL_HITL_INFRARED) {
//		    /** This code simulates infrared.c:ir_update() */
//		    ir_roll = DL_HITL_INFRARED_roll(dl_buffer);
//		    ir_pitch = DL_HITL_INFRARED_pitch(dl_buffer);
	} 
	public void encode_hitl_ubx(UplinkDevice d, int acId, int ubx_class, int ubx_id, 
						    long payload[])
	{
//			else if (msg_id == DL_HITL_UBX) {
//		    /** This code simulates gps_ubx.c:parse_ubx() */
//		    if (gps_msg_received) {
//		      gps_nb_ovrn++;
//		    } else {
//		      ubx_class = DL_HITL_UBX_class(dl_buffer);
//		      ubx_id = DL_HITL_UBX_id(dl_buffer);
//		      uint8_t l = DL_HITL_UBX_ubx_payload_length(dl_buffer);
//		      uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(dl_buffer);
//		      uint8_t i;
//		      for(i=0; i<l; i++) {
//			ubx_msg_buf[i] = ubx_payload[i];
//		      }
//		      gps_msg_received = TRUE;
//		    }
	}
	public void encode_set_var(UplinkDevice d, int acId, int var, float val)
	{
		ByteBuffer payload=ByteBuffer.allocate(8);
		payload.order(ByteOrder.LITTLE_ENDIAN);
		payload.put((byte)(0x99));
		payload.put((byte)4); // D/L SETTING msg
		payload.put((byte)var); //index
		payload.put((byte)0); //pad0??
		payload.putFloat(val); //value
		try {
			d.sendTX(payload.array(), acId);
		} catch (IOException e) 
		{
			System.out.println("IOError sending var="+String.valueOf(var)+
					" val="+String.valueOf(val)+
					" to "+String.valueOf(acId));
		}
	}
}
